#include "publisher/publisher.h"

namespace publisher {

Publisher::Publisher(const int& trajectory_id) {
 	poses_.clear();
 	gt_poses_.clear();
 	odometry_poses_.clear();
 	current_landmarks_.clear();
 	filter_landmarks_.clear();
}

void Publisher::SetOdometryPose(const Eigen::Vector3d& world_pose) {
    std::lock_guard<std::mutex> lock(odometry_mutex_); 
	odometry_poses_.push_back(world_pose);
}    

void Publisher::SetCurrentLandmarks(const std::vector<Eigen::Vector3d>& landmark) {
	std::lock_guard<std::mutex> lock(current_landmark_mutex_); 
	current_landmarks_ = landmark;
}

void Publisher::SetState(const Eigen::Vector3d& world_pose) {
	std::lock_guard<std::mutex> lock(state_mutex_); 
	poses_.push_back(world_pose);
}

std::vector<Eigen::Vector3d> Publisher::get_poses() {
	std::lock_guard<std::mutex> lock(state_mutex_); 
	return poses_;
}

std::vector<Eigen::Vector3d> Publisher::get_gt_poses() {
	return gt_poses_;
}

std::vector<Eigen::Vector3d> Publisher::get_odometry_poses() {
	std::lock_guard<std::mutex> lock(odometry_mutex_); 
	return odometry_poses_;
}

std::vector<Eigen::Vector3d> Publisher::get_current_landmarks() {
	return current_landmarks_;
}

std::vector<Eigen::Vector3f> Publisher::get_filter_landmarks() {
	std::lock_guard<std::mutex> lock(filter_point_cloud_mutex_); 
	return filter_landmarks_;
}

std::vector<Eigen::Vector3d> Publisher::get_submap_landmarks() {
	std::vector<Eigen::Vector3d> submap_landmarks;

	//std::cout << " submap_landmarks : " << submap_landmarks.size() << std::endl;
	return submap_landmarks;
}

std::vector<Eigen::Vector3d> Publisher::get_all_poses() {
	return all_poses_;	
}

} // namespace publisher
